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ABSTRACT 


The  major  problem  addressed  by  this  research  is  how  to  develop  a  motion  control 
algorithm  for  local  motion  planning  of  an  autonomous  mobile  robot. 

The  approach  taken  was  to  clearly  define  the  robot’s  motion  descriptions  and  to  design 
a  high-level,  machine  independent  robot  control  language  called  MML  (Model-based 
Mobile  robot  Language). 

The  results  are  that  the  robot  can  implement  smooth  rotation,  symmetric,  and  initial 
motion  tracking  on  an  given  environment.  Based  on  the  motion  control  algorithm 
developed  in  this  thesis,  the  robot  with  rigid  body  can  be  applicable  in  both  local  and  global 
motion  planning. 

The  experimental  results  were  successful.  The  algorithms  were  implemented  first  on 
a  simulator,  then  on  the  autonomous  mobile  robot  Yamabico-11 .  Given  an  initial  and  final 
configuration,  the  vehicle  demonstrated  the  capability  to  safely  achieve  its  goal. 
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1.  INTRODUCTION 


One  of  the  ultimate  goals  in  robotics  is  to  develop  an  autonomous  robot  -  being  capable  of 
successfully  completing  a  task  given  what  to  do  and  how  to  do  it.  Increasingly  capable  autono¬ 
mous  robot  platforms  are  being  developed  to  handle  a  myriad  of  hazardous  duty  assignments. 
While  manufacturing  dominates  the  area  of  robotic  applications,  useful  advances  have  been  made 
in  the  areas  of  waste  management,  space  exploration,  undersea  work,  assistance  for  the  disabled 
and  medical  surgery  [Lato].  Several  government-sponsored  efforts  are  underway  for  building  sys¬ 
tems  for  military  applications  such  as  fighting  fires,  handling  ammunition,  transporting  material, 
conducting  underwater  search  and  inspection  operations,  and  other  dangerous  tasks  now  per¬ 
formed  by  humans  [Nava]. 

Many  of  the  above  tasks  require  motion  of  the  robot  in  order  to  carry  out  any  task.  This 
problem  is  known  as  the  motion  planning  problem.  We  divide  the  motion  planning  problem  into 
two  sub-problems:  the  global  and  local  motion  planning.  The  robot  motion  can  be  described  as 
follows:  a  path  of  robot  to  carry  out  a  task  while  moving  from  one  configuration  (p,  0,  k)  to 
another.  In  motion  planning,  not  only  is  its  position  important,  but  the  orientation  and  curvature  of 
the  robot  are  important  as  it  follows  the  path. 

The  purpose  of  the  research  is  to  investigate  fundamental  theories  for  navigation  to  con¬ 
struct  autonomous  mobile  robot  for  military  and  industrial  applications.  In  this  thesis,  we  will  dis¬ 
cuss  several  local  motion  planning  problems  inevitable  for  the  robot  to  carry  out  any  task.  They 
are  divided  as  follows:  smooth  rotation,  symmetric,  and  initial  motion  planning. 


1 


II.  BACK  GROUND 


The  focus  of  this  thesis  is  on  planning  the  motion  of  an  autonomous  mobile  robot.  The 
algorithms  for  motion  planning  to  be  taken  by  real  robot  are  developed.  Many  simulations  are 
done  to  verify  the  algorithms. 

A  high-level  motion  specification  language.  Mobile-based  Mobile-robot  Language 
(MML- 11),  suitable  to  describe  the  solutions  to  the  motion  planning  problem  is  used  as  the  imple¬ 
mentation  and  verification  of  the  algorithms. 

The  world  space  “W^for  the  motion  planning  problem  in  this  thesis  is  a  two  dimensional 

plane  on  which  a  global  Cartesian  coordinate  system  is  defined.  The  obstacles  are  closed  sub¬ 
sets  of  W.  The  free  space,  FREE(  W),  is  the  complement  of  the  union  of  all  obstacles  in  the  world 
space  W 

The  vehicle  in  the  thesis  refers  to  a  rigid  body  robot  which  has  fixed  shape  -  during  the 
research  we  will  use  Yamabico-11  for  experiments  although  the  algorithms  will  be  suitable  for 
robots  with  any  variable  shape. 

A  configuration  q  in  this  thesis  defined  as  a  combination  of  position,  orientation,  and  cur¬ 
vature,  (p,  0,  k),  where  p  indicates  the  position  (x,  y)  in  the  global  Cartesian  coordinate  system,  0 
is  the  orientation  related  to  the  x-axis  of  the  global  coordinate  system  [Loza],  and  k  is  the  speci¬ 
fied  curvature.  The  configxiration  defined  in  this  thesis  is  normally  used  to  describe  the  robot’s 
instantaneous  status,  either  it  is  stationary  or  moving.  This  configuration  is  specially  useful  to 
specify  a  path.  For  instance,  if  we  use  q  —  {(x,  y),  0,  k)  to  specify  a  line,  this  line  passes  through 
the  point  at  (x,  y)  and  with  orientation  0.  When  the  curvature  element  k=0,  it  is  specifying  a 
straight  line,  otherwise  it’s  a  circle. 
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The  motion  of  a  vehicle  is  subject  to  nonholonomic  and  kinematic  constraints,  that  is,  the 
vehicle  is  able  to  perform  both  forward  and  backward  motion  but  not  sideways  motion.  The 
robot’s  orientation  and  curvature  at  the  path  are  continuous. 
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III.  PROBLEM  STATEMENTS 


The  problem  investigated  in  this  thesis  is  intelligent  behavior  of  autonomous  mobile 
robots  with  emphasis  on  local  motion  planning  and  how  to  precisely  and  stably  control  the 
motions  of  the  autonomous  mobile  robot  Yamabico-11.  This  problem  is  subdivided  into  the  fol¬ 
lowing  problems: 

1.  What  procedures  should  an  autonomous  mobile  robot  follow  in  order  to  rotate  smoothly 
in  a  given  environment? 

2.  What  procediures  should  an  autonomous  mobile  robot  follow  to  get  to  the  configuration 
of  not  being  on  the  center  of  the  path? 

3.  What  procedures  should  an  autonomous  mobile  robot  follow  to  avoid  the  collision  of 
the  walls  and  to  get  to  the  goal  configuration? 
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IV.  SMOOTH  ROTATION 


A.  MOTION  DESCRIPTION 

We  consider  a  situation  where  the  vehicle  at  an  arbitrary  configuration  qs  =  (ps,  6i,  ks)  is 
supposed  to  rotate  its  orientation  from  0^  to  an  arbitrary  9g  without  changing  its  position  ps.  (Fig¬ 
ure  1) 


Figure  1:  Rotating  of  orientation  without  changing  its  position 


The  motion  of  this  vehicle  can  be  described  as  follows: 

Start  rotating  the  orientation  towards  the  desired  direction. 

Check  the  vehicle  heading. 

Stop  rotating  when  its  heading  equals  to  the  goal  orientation. 

As  we  can  see,  the  motion  does  not  involve  any  translation  of  the  vehicle.  So  it  is  not  nec- 
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essary  to  use  the  steering  function.  Instead,  the  motion  is  based  on  relation  between  difference  of 
angles  and  rotational  speed  of  the  vehicle.  We  need  an  algorithm  which  controls  the  rotational 
speed  of  the  vehicle  depending  on  the  vehicle’s  rotational  angle. 

B.  MOTION  PLANNING 

1.  Algorithm  For  Smooth  Rotation 

The  algorithm  as  Eq  4. 1  is  a  tool  for  a  mobile  robot  vehicle  performing  smooth  rotation. 

^  =  -2A:w-^^e^-ep  (Eq4.1) 

where  ^ :  Vehicle’s  rotational  acceleration, 

dt 

w:  Vehicle’s  rotational  speed, 

6^:  Vehicle’s  initial  orientation, 

0?;  Vehicle’s  goal  orientation, 
k:  Positive  constant 

As  we  can  see  in  Eq  4. 1,  the  rotational  speed  is  the  only  control  variable  of  the  algorithm. 
While  rotating,  the  algorithm  returns  rotational  acceleration  depending  on  the  current  speed  at 
each  time,  and  hence,  the  speed  of  the  vehicle  also. 

2.  Motion  Planning  With  Algorithm 

In  Eq  4.1,  the  positive  constant  k  is  an  important  factor  of  the  algorithm.  The  bigger  k  we 
use,  the  rotational  time  is  shorter,  and  the  smaller  k,  it  takes  more  time  to  rotate.  Figure  2  shows 
the  relation  between  speed  and  time  for  the  vehicle  to  rotate  %  degrees  with  various  k. 


8 


TIME  (t) 


Figure  2:  The  graph  for  smooth  rotation  algorithm 


As  shown  in  the  figure  2,  using  bigger  k,  for  example,  A:  =  4  in  the  graph,  gives  the  vehicle 
higher  speed  and  shorter  time  to  complete  its  mission  But  in  some  cases,  it  can  be  dangerous  for 
the  vehicle  if  the  speed  exceeds  the  vehicle’s  speed  limit.  There  can  be  no  such  high  speed  in 
using  smaller  k,  but  it  has  two  defects:  one  being  too  much  time  to  do  its  job,  and  the  other  being 
the  lower  speed  duration  time  is  too  long. 
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c. 


EXPERIMANTAL  RESULT 


Considering  rotational  time,  we  used  ^  =  4  curve,  but  for  the  safety  of  the  vehicle,  we  limit 
the  speed  with  w  =  3.  Figure  3  shows  the  result. 


TIME  (t) 


Figure  3:  The  graph  for  redefined  algorithm 


As  shown  by  the  result,  applying  the  speed  limit  makes  the  algorithm  more  useful. 


V.  SYMMETRIC  MOTION  PLANNING 


A.  MOTION  DESCRIPTION 

We  consider  a  situation  where  the  vehicle  is  supposed  to  track  a  path,  and  to  get  to  the  goal 
position  which  may  not  be  on  the  center  of  the  path.  A  vehicle  is  represented  by  a  point  ps  =  (xs, 
ys),  its  orientation  Os,  and  curvature  ks,  and  the  goal  position  is  represented  by  p*  =  (xg,  yg),  and  its 
orientation  a«,  and  its  curvature  kg.  Figure  4  shows  the  situation. 


I _ I 


Figure  4:  Symmetric  motion 

The  motion  of  this  vehicle  can  be  divided  into  two  parts;  one  being  the  motion  on  the  path 

A,  and  the  other  being  the  motion  on  the  ciuwe.B.  The  first  part  of  the  motion  is  easily  done  by  the 
steering  function,  but  the  motion  on  the  curve  B  is  difficult  to  make  it  using  the  transformation. 
For  the  safety  of  vehicle,  it  is  important  to  keep  moving  on  the  center  of  path  as  long  as  possible. 

B.  MOTION  PLANNING  WITH  STEERING  FUNCTION 
1.  Line  IVacking 

Let’s  assume  a  directed  straight  line  L  represented  by  a  point  (a,  b)  on  it,  and  its  orienta- 
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tion  a.  A  vehicle  away  from  the  line  is  supposed  to  track  the  line.  The  vehicle  is  represented  by  p 
=  (x,  y),  and  its  orientation  6.  (Figure  5) 


Figure  5:  Line  tracking 

The  steering  function  for  the  vehicle  supposed  to  track  the  line  L  is  [Kana] 

Hk 

—  =  -(ak  +  b(Q-a)  +c(-  (x-a)  sina+  (y-b)  cosa))  (Eq5.1) 

as 

Using  this  steering  function,  a  vehicle  tracks  a  dotted  line  as  shown  in  Fig.  5.2.  How  the 
steering  function  accomplishes  a  line  tracking  is  fully  described  in  [Kana],  [Macp],  and  [Kova]. 

2.  Symmetric  Motion 

As  seen  in  Section  A  in  this  Chapter,  the  motion  B  is  a  reverse  action  of  line  tracking. 
Let’s  assume  a  virtual  vehicle  which  tracks  a  line  from  goal  position.  There  will  be  a  path  kept  by 
the  virtual  vehicle.  With  the  path  in  the  reverse  direction,  the  real  vehicle  can  move  toward  the 
goal  position. 

a.  Virtual  Vehicle  Simulation 
We  let 
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Start  Configuration:  qs  =  ((xs,  ys),  qs,  ks). 

Goal  Configuration:  qg  =  ((xg,  yg),  qg,  kg) 

Let’s  assume  there  is  a  virtual  vehicle  at  the  goal  position  with  reverse  direction. 
The  virtual  vehicle  is  supposed  to  track  a  directed  straight  line  L  represented  by  the  start  configu¬ 
ration  qs. 


I 


Figure  6:  Virtual  vehicle  tracking 


We  can  get 
kd=  -ks, 

0d  =  0s  7t, 

Ad  =  -(x  -  Xs)sin0d  +  (y  -  ys)cos0d, 
because  the  virtual  vehicle  uses  the  qs  as  its  goal  configuration. 

The  virtual  vehicle  steering  function  is 

^  =  -(ak  +  b(Q-Q^)  +  c (- (x-x^)  sin0^-i-  (y-y,)  cos0^) )  (Eq5.2) 

Using  this  steering  function,  the  virtual  vehicle  tracks  the  lineL,  and  finally  gets  to 
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the  start  configuration.  Configurations  generated  during  the  virtual  vehicle  move  are  stored  in 
array  to  be  used  by  real  vehicle  later.  When  storing  configurations,  6  is  changed  to  6  +  ji,  and 
kappa  is  changed  its  sign(+/-). 

b.  Real  Vehicle  Moving  while  Simulation 

While  the  virtual  vehicle  simulates  a  line  tracking,  the  real  vehicle  moves  forward 
also.  There  will  be  some  point  between  start  configuration  qs  and  goal  configuration  qg  where  the 
real  vehicle  meets  the  virtual  one.  The  point  has  an  important  role  in  symmetric  motion  planning, 
because  the  virtual  vehicle  finishes  its  line  tracking  simulation,  and  the  real  vehicle  starts  tracking 
a  simulated  path  at  the  point.  We  need  to  know  how  to  get  the  point. 


qs  9^ 


Figure  7:  Real  vehicle  moving 

As  shown  in  Figure  7,  we  can  get  the  distance  between  the  real  vehicle  and  the 
virtual  one  by  computing  the  local  coordinate  x*  -  detailed  explanation  for  the  local  coordinate 
will  be  discussed  in  later  subsection.  When  the  distance  equals  to  0,  it  is  the  time  the  real  vehicle 
starts  tracking  a  simulated  path  generated  by  virtual  vehicle. 
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c. 


Local  Coordinate 


We  take  simulated  configurations  by  the  virtual  vehicle  sparser  than  those  gener¬ 
ated  by  the  real  vehicle.  In  addition  to  the  data,  the  real  vehicle  uses  the  steering  function  locally 
between  the  configurations. 

Let’s  assume 

Vehicle  configuration:  q  =  ((x,  y),  0,  k). 

Target  configuration:  qc  =  ((xc,  yc),  dc.kc) 

To  use  the  steering  function,  we  need  to  compute  a  local  coordinate  q*  =  ((x*,  y*),  0*,  k*) 
between  vehicle  and  target  configuration.  (Figure  8) 


Figure  8:  Local  coordinate 

The  step  how  to  compute  local  coordinate  q*  is  as  follows: 
qc  =  q  o  q*, 
q’  o  qc  =  (q’  o  q)  o  q* 


As  a  result,  we  get 

q*  =  q’  o  qc 
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(Eq5.3) 


W^th  the  Eq  5.3,  we  can  compute  local  coordinates  as  follows  [Kana95] 

X*  =  (x  -  Xc)cos0  +  (y  -  yc)sin0  (Eq  5.4) 

y*  =  (x  -  Xc)sin0  +  (y  -  yc)cos0  (Eq  5.5) 

d.  Real  Vehicle  Tracking 

As  shown  in  Fig.  5.4,  the  real  vehicle  uses  a  configuration  of  the  simulated  path 
one  by  one  as  its  target.  The  step  how  to  track  the  simulated  path  is  as  follows: 

(1)  Compute  local  coordinate  between  vehicle  and  target  configuration 

(2)  Check  X* 

(2.1) Ifx*<=0 

(2.1.1)  If  the  target  is  goal  configiu:ation 

(2. 1.1.1)  Stop  vehicle 

(2.1.2)  If  the  target  is  not  goal  configuration 

(2. 1.2.1)  Change  the  target  with  a  new  configuration  of  the  simulated  path 

(2.2) Ifx*>0 

(2.3)  Move  the  vehicle  using  steering  function 

The  real  vehcle  uses  the  steering  function  as  follows: 

-(aAk  +  M0  +  cAd)  (Eq  5.6) 

Ak  =  k  -  kc, 

A0  =  0  -  00, 

Ad  =  y* 


ds 

where 


16 


C.  EXPERIMENTAL  RESULT 

Figure  9  shows  the  result  in  which  the  real  vehicle  uses  the  same  smoothness  as  the  virtual 
vehicle  does.  The  vehicle’s  initial  configuration  is  (0, 0, 0).  The  vehicle’s  goal  position  is  (400, 


Figure  9:  The  result  with  the  same  a  as  the  virtual  vehicle  does 


Figure  10  shows  the  result  in  which  the  real  vehicle  uses  one  half  smoothness  of  the  vu:- 
tual  vehicle.  The  vehicle’s  initial  configuration  is  (0, 0, 0).  The  vehicle’s  goal  position  is  (400, 60) 
and  its  orientation  is  0  degrees.  As  a  result,  the  real  vehicle  gets  to  the  configiuration  (400, 60.5, 0). 


Sinulated.Path' 


Figure  10:  The  result  with  a  half  of  virtual  vehicle  a 
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Figure  1 1  shows  the  result  in  which  the  real  vehicle  uses  one  quarter  smoothness  of  the 
virtual  vehicle.  The  vehicle’s  initial  configmation  is  (0, 0, 0).  The  vehicle’s  goal  position  is  (400, 
60)  and  its  orientation  is  0  degrees.  As  a  result,  the  real  vehicle  successfully  gets  to  the  configura¬ 
tion  (400, 60, 0). 


0  50  100  150  200  250  300  350  400  450 


Figure  11:  The  result  with  a  quarter  of  virtual  vehicle  a 
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VL  INITIAL  MOTION  PLANNING 

A.  MOTION  DESCRIPTION 

We  consider  a  situation  where  the  vehicle  at  an  arbitrary  configuration  qs  =  (ps,  0^,  h)  is 
located  on  the  world  which  is  consisted  of  walls.  There  is  one  border  which  has  a  configuration 
called  Exit  configuration  qe  =  (pe,  6e,  ke).  Figure  12  illustrates  this  situation. 


Figure  12:  The  case  where  simulated  path  collides 

The  world  where  vehicles  are  located  is  a  part  of  k-regions  [Kana95][Kova].  A  vehicle 
located  in  the  world  has  a  mission  to  find  a  path  which  can  converge  to  the  exit  configuration  qe 
without  colliding  the  wall.  The  mission  is  a  kind  of  line  tracking,  that  is,  the  vehicle  with  a  config¬ 
uration  qs  is  supposed  to  track  a  straight  line  qref  represented  by  pe  and  its  orientation  6c  using 
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steering  function.  As  shown  in  the  figure,  since  the  vehicle  at  configuration  qsi  is  too  close  to  the 
wall,  the  vehicle  will  collide  the  wall.  For  the  safety  of  the  vehicle,  this  behavior  can  not  be 
accepted.  The  other  vehicle  at  configuration  qsi  is  far  away  from  the  wall,  but  the  vehicle  fails  to 
pass  the  exit  configuration  qe.  The  exit  configuration  qe  is  the  start  configuration  of  next  region. 
It’s  important  for  the  vehicle  to  get  to  the  exact  configuration.  The  mission  can  be  divided  into 
two  parts:  one  being  the  mission  to  find  a  path  to  avoid  colliding  of  walls,  and  the  other  being  the 
mission  to  find  a  path  to  get  to  the  exit  configuration  qe. 

B.  MOTION  PLANNING  WITH  STEERING  FUNCTION 

The  steering  function  (Eq  6.1)  is  used  for  a  vehicle  to  perform  smooth  line  tracking. 

—  =  -  {A^k  +  5  A0  +  CM)  (Eq  6.1) 

ds 

In  Eq  6.1,  A,  B,  and  C  are  positive  constants  which  are  related  to  the  smoothness  of  vehi¬ 
cle’s  motion  [Kana].  They  are  determined  by  the  smoothness  a  as  follows: 


k=l/a 

(Eq  6.2) 

> 

II 

CO 

* 

(Eq6.3) 

B=3*k*k 

(Eq  6.4) 

C=3*k*k*k 

(Eq  6.5) 

From  the  above  equations,  we  know  the  smoothness  <7  determines  the  sharpness  of  the 
tracking  trajectory.  We  can  see  that  the  smaller  the  value  of  smoothness  is,  the  sharper  the  trajec¬ 
tory  will  be,  and  the  larger  the  smoothness  is,  the  longer  the  vehicle  travels  [Kana]. 

While  we  know  tracking  trajectory  depends  on  smoothness,  it  is  important  to  know  that  a 
lower  limit  of  smoothness  exists  in  using  the  steering  function  to  track  a  line.  The  lower  limit  of 
smoothness  is  as  follows  [Chuang]: 

a  =  0.095*Adimt  (Eq6.6) 
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In  Eq  6.6,  Adinit  is  the  closest  distance  from  the  initial  configuration  to  the  reference  line.  If 
a  smaller  smoothness  a  than  the  lower  limit  is  applied,  the  tracking  trajectory  never  converge  to 


the  reference  line. 

In  addition  to  the  lower  limit  of  smoothness,  we  have  to  consider  there  will  be  undesirable 
tracking  trajectories  to  converge  to  the  reference  line  with  some  arbitrary  smoothness.  Although 
they  converge  to  the  reference  line,  the  trajectory  travels  backward.  The  minimum  desirable 
smoothness  for  parallel  fine  tracking  is  as  follows  [Chuang]: 

a  =  0.18*  Adinit  (Eq6.7) 

1.  Motion  Planning  to  Avoid  The  Collision 

A  fine  tracking  motion  starts  from  a  configuration  qs  =  (ps,  0^,  ks).  The  goal  of  the  fine 
tracking  is  the  line  called  reference  fine  qr^,  specified  by  a  configuration  qe  =  (pe,  6e,  ke).  Since  the 
fine  is  a  straight  line,  it  has  a  constant  curvature  ke  =  0.  Figure  13  shows  the  case. 


Figure  13:  The  case  to  avoid  collision 
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The  path  of  line  tracking  in  the  figure  refers  to  a  straight  line  qref.  But  we  can  make  the  Eq 
6. 1  simpler  using  qref  as  a  reference  line  instead  of  using  qref.  We  can  use  a  bigger  smoothness  to 
converge  to  the  reference  line,  because  the  Arf  is  0. 

—  =  -(AAk  +  BAe)  (Eq6.8) 

as 

The  step  to  avoid  collision  is  as  follows; 

(1)  Start  line  tracking  simulation  with  default  smoothness 

(2)  Check  if  path  collides  the  wall 

-  Checking  collision  will  be  discussed  later  in  this  chapter 

(2. 1)  If  the  path  collides  the  wall 

(2.1.1)  Change  the  smoothness  to  a  smaller  one 

(2. 1.1.1)  Check  if  the  smoothness  is  smaller  than  0.095  *  isdimt 

(2. 1 . 1 . 1 . 1 )  Quit  the  simulation 

(2. 1.1. 1.2)  No  path  to  avoid  collision  exists 

(2.1.2)  Start  line  tracking  simulation  again  with  new  smoothness 

(2.2)  If  there  is  no  collision  on  the  path 

(2.2.1)  Check  if  the  path  is  parallel  to  the  exit  configmation  qe 

(2.2.2)  If  the  path  is  parallel  to  the  exit  configuration,  stop  the  simulation. 

(3)  Return  the  smoothness  o 

2.  Motion  Planning  to  Converge  to  The  Reference  Line 

A  line  tracking  motion  starting  from  a  configuration  qs  =  (ps,  0^,  ks)  reached  to  a  configura¬ 
tion  qr,  =  (pn,  0n,  kn)  wWch  is  parallel  to  the  exit  configiuration  qe.  From  the  configuration  qn  =  (pn, 
0/1,  kn),  another  line  tracking  motion  starts  to  converge  to  the  reference  line  specified  by  qe  = 
(pe,  0e,  ke).  Figurc  14  shows  this  case. 
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Figure  14:  The  case  to  converge  to  the  reference  line 

The  path  of  line  tracking  in  the  figure  refers  to  a  straight  line  qref.  A  line  tracking  starts 
with  default  smoothness  a  again.  The  reason  not  using  smoothness  which  we  got  in  Section  1  is 
that  the  simulated  path  could  be  sharper  to  avoid  the  collision,  that  is,  the  smoothness  would  be 
smaller  than  0.18  *  6dimt.  A  line  tracking  path  with  the  smoothness  will  be  undesirable  to  con¬ 
verge  to  the  reference  line  -  the  detailed  explanation  is  discussed  in  Section  1  in  this  Chapter. 
The  step  to  converge  to  the  reference  line  is  as  follows: 

(1)  Start  line  tracking  simulation  with  default  smoothness 

(2)  Check  if  x*  equals  to  0,  or  smaller  than  0 

(1.1)  Check  if  y*  equals  to  0 

(1.1.1)  Return  the  smoothness  G 

(1.1.2)  Quit  the  simulation 
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(1.2)  If  y*  does  not  equal  to  0 


(1.2.1)  Change  the  smoothness  to  a  smaller  one 

(1.2.2)  Check  if  the  smoothness  is  smaller  than  0.18  *  Mimt 

(1. 2.2.1)  Quit  the  simulation 

(1. 2.2.2)  No  path  exists 

(3)  Start  line  tracking  simulation  again  with  new  smoothness 

3.  Motion  Planning  to  Determine  The  Direction  of  Path 
A  line  tracking  motion  starting  from  a  configuration  qs  =  (ps,  Qs,  ks)  will  get  to  the  goal 
configuration  qe  =  (pe,  0e,  ke)  on  one  desirable  path.  But  actually,  regarding  to  the  heading  of  the 
vehicle,  there  can  exist  more  desirable  path.  Figure  15  shows  this  case. 


Figure  15:  The  case  of  various  direction  of  paths 


The  direction  of  the  path  is  related  not  only  to  the  heading  of  the  vehicle  0s,  but  to  the 
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orientation  of  the  Exit  configuration  6^. 

The  step  to  determine  the  direction  of  the  path  is  as  follows: 

(A)  IO(0s-0e)l<=Jt/2 

-  <I>  means  normalization 

(1)  If  ((0s  -  0e)  > 7t/2)  Then  0  =  0s  -  2  jc 

(2)  Else  if  ((0s  -  0e)  <  7t  /  2)  Then  0  =  0s  -  2  it 

(B)  ld)(0s-0e)l>Ji/2 

(1) y*>=0 

(a)  If  ((0s  -  0e)  >  0)  Then  0  =  0s  -  2  it 

(b)  Else  if  ((0s  -  0e)  <  -2  it)  Then  0  =  0s  -  2  it 

(2)  y*<0 

(a)  If  ((0s  -  0e)  >  2  It)  Then  0  =  0s  -  2  it 

(b)  Else  if  ((0s  -  0e)  <  0)  Then  0  =  0s  +  2  it 

4.  Motion  Planning  to  Check  Collision 

In  a  motion  planning  simulation  from  a  configuration  qs  =  (ps,  0i,  ks)  to  find  a  desirable 
path  to  get  to  the  exit  configuration  qe  =  (pe,  Qe,  ke),  it  is  important  to  check  if  the  path  collides  the 
wall.  To  test  it,  we  need  to  know  how  the  world  is  described, 
a.  Inverted  World 

We  consider  a  two-dimensional  world  W  with  holes.  A  hole  is  an  obstacle  for  a 
vehicle.  A  free  space  Free(W)  is  the  complement  of  the  union  of  all  holes.  There  might  be  a  hole 
among  them  which  completely  surrounds  the  free  space.  The  hole  like  this  is  said  to  be  inverted. 
Every  free  space  is  a  connected  subset  of  W.  Figure  16  shows  this  world. 
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b.  View  Angle 

With  a  polygon  B  and  a  point  p  which  is  not  on  its  boundary,  the  view  angle  yi{p)  of 
the  /th  edge  v,  (p(v)  at  p  is  defined  as  follows  (Figure  17) : 

Y,.  (p)  =  ^>  ('F  (p,  (p  (V;) )  -  'F  (p,  )  (Eq  6.9) 
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The  sum  of  the  n  view  angles  in  B  is  defined  as 

n 

rs(p)  =  (Eq6.10) 

(=1 

In  the  inverted  world,  if  FbO?)  =  -27t,  the  point  p  is  in  Free(W),  otherwise, 
it’s  not  in  Free(W)  [Kana]. 

c.  Collision  Test  of  Vehicle 

So  far,  the  vehicle  in  the  motion  planning  has  been  dealt  as  if  one  point.  But  it  is 
not  enough  to  test  if  the  vehicle  collides  the  walls.  In  real  world,  the  vehicle  has  a  rigid  body  with 
size  such  as  width  and  length.  There  might  be  a  situation  while  one  corner  of  the  vehicle  is  in  the 
free  space,  the  other  corner  could  collide  the  wall.  This  behavior  can  not  be  accepted.  We  need  to 
know  how  to  test  collision  of  each  comer  of  the  vehicle  at  every  step  of  the  motion  planning. 


wall 


As  shown  in  the  Figure  1 8,  we  know  two  important  informations  such  that  the  con¬ 
figuration  of  the  center  of  the  vehicle  q  which  is  on  the  motion  planning  path,  and  the  local  coor¬ 
dinates  of  vehcle’s  corners  such  as  cl,  c2,  c3,  c4,  c5.  With  the  information,  we  can  get  each 
comer’s  configuration  as  follows: 
ql  =qocl, 
q2  =  q  o  c2, 
q3  =  q  o  c3, 
q4  =  q  o  c4, 
q5  =  q  o  c5 

Instead  of  testing  center  of  the  vehicle,  the  collision  test  will  be  doing  at  each  cor¬ 
ner. 
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C.  EXPERIMENTAL  RESULT 

Figure  19  shows  the  result  in  which  the  vehicle  located  at  a  configuration  qs  =  ((50.0, 
100.0),  -3jr/4, 0.0)  finds  a  path  to  converge  to  the  exit  configuration  Qe  =  ((175.0, 679.704),  7t/2 
0.0)  avoiding  a  collision  of  the  walls.The  default  smoothness  a  is  80.  The  vehicle  used  in  the 
experiment  has  the  size:  width  =  50cm,  length  =  60cm. 


APPENDIX 


A.  C  PROGRAM  FOR  SMOOTH  ROTATION 


Function  :  rotateRule() 

Purpose  :  updates  the  commanded  velocity  to  rotate  the  robot 
Parameters:  VELOCITY  actual,  commanded 
Returns  :  The  required  linear  velocity,  rotational  velocity 
Called  by : 

Calls  :  limitO 

4c  :i:  *  *  H€  *  *  *  *  *  Jf:  Jii  *  *  *  5fi  :jc  5k  *  3}:  5ii  *  *  *  Ji:  Jf!  *  *  *  *  *  *  *  Jis  *  JfJ  >!«  *  *  He  *  *  5f:  Jf:  5k  *  *  *  *  sf«  Jfi 


static  VELOCITY 

rotateRuIe(VELOCITY  actual,  VELOCITY  commanded) 

{ 

double  k  =  0.5; 
static  double  goalTheta; 
static  int  noGoalTheta  =  TRUE; 
static  double  rotVel  =  0.0; 
double  rotAcc  =  0.0; 

if  (noGoalTheta)  { 

/*  set  the  goal  of  rotation  */ 

goalTheta  =  vehicle.Theta  +  currentPath.config.Theta; 
noGoalTheta  =  FALSE; 


commanded.Linear  =  0.0; 

if  (fabs(vehicle.Theta  -  goalTheta)  >=  0.005)  { 
rotAcc  =  -2  *  k  *  rotVel  -  k  *  k  * 

(vehicle.Theta  -  goalTheta); 
rotAcc  =  limit(rotAcc,  DEFAULT_ROT_ACC); 
rotVel  =  rotVel  +  rotAcc  *  MOTION_CONTROL_CYCLE; 
rotVel  =  Umit(rotVel,  DEFAULT_GOAL_VEL_ROT); 
commanded.Rotational  =  rotVel; 

} 

else  {  /*  the  rotation  is  completed  */ 
currentPath.pathType.mode  =  STOPMODE; 
commanded.Rotational  =  0.0; 
noGoalTheta  =  TRUE; 
rotVel  =  0.0; 

} 

return  commanded; 
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Function  :  limit 

Purpose  ;  This  function  is  used  by  rotate  function 
to  limit  rotational  speed  of  the  robot 
Parameters  :  double  y,  1 
Returns  :  double  y,  1 
Called  by  :  rotateRuleQ 
Calls  :  None 


double 

limit(double  y,  double  1) 

{ 

if(y>l)y  =  l; 
else  if  (y  <  -1)  y  =  -1; 
return  y; 

} 
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B.  C  PROGRAM  FOR  SYMMETRIC  MOTION  PLANNING 


<ti^  •Ji*  *3^  ^2#  ^1#  fc|t|^  <f^  ^2^  aL*  aI^  a^  a^  a^  Alf 

/  ^Jy  ^j>  ffw  ^|A  Jp  #ji  ^|v  iJC  ijt  ^j?  J'IS  <J»  #Jv  ^|a  •'!>  ^J>  ¥^  •JA  *X*  ^X*  ^T* 

Module  Name;  Header  file 
Purpose:  define  variables 
Parameters; 

Returns: 

Called  by: 

Calls: 

jjc  jjc  ^|c  jjc  jjc  5|c  jjc  jjc  ^jc  ^c  jfc  ^f»  jjc  5f»  jjc  ^|c  jjc  jjc  jjc  ^c  ^1?  jjc  ^Jc  ^c  ^c  ifc  jJa  i'Jc  jfi  jjc  jjc  ijc  ^c  5jc  ij?  jjc  ^c  jjc  jjc  5|c  jfc  ijc  jjc  jjc  jJj  j 


#include  <iostream.h> 
#include  <math.h> 


#define  PI  3.1415926 


typedef  struct  { 
double  X; 
double  Y; 

}POINT; 

typedef  struct  { 

POINT  Posit; 
double  Theta; 
double  Kappa; 

ICONHGURATION; 

CONHGURATION  symConfig[100000]; 

/*  smoothness  for  virtual  and  real  robot  */ 
double  sigma  =  20; 
double  sigma2  =  1; 

/*  different  delta  s  is  used  in  calculatedVirtualRobotPath()  */ 
double  deltas  =0.1; 
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/*  constant  for  steering  function  used  in  simulation  */ 

double  k  =  1.0  /  sigma; 

double  a  =  3  *  k; 

double  b  =  3  *  k  *  k; 

double  c  =  k  *  k  *  k; 

/*  constant  for  steering  function  used  in  real  robot  */ 

double  kk  =  1.0  /  sigma2; 

double  aa  =  3  *  kk; 

double  bb  =  3  *  kk  *  kk; 

double  cc  =  kk  *  kk  *  kk; 

/*  define  functions  used  in  the  program  */ 

CONHGURATION  moveRealRobot(CONFIGURATION  q); 

CONFIGURATION  calculateVirtualRobotPath(CONFIGURATION  q,  CONFIGURATION  L); 
CONFIGURATION  moveRobotOnSimulatedPath(CONnGURATION  q,  int  index); 
double  computeSymmetricPathKappa(CONFIGURATION  q,  int  i); 
double  getKappa(CONFIGURATION  q,  CONFIGURATION  L); 

/*  Library  functions  */ 

CONHOURAHON  computeQstar(CONFIGURATION  ql,  CONHGURATION  q2); 
CONHGURATION  composite(CONFIGURATION  ql,  CONHGURATION  q2); 
CONHGURATION  circ(double  deltaS,  double  theta); 
double  norm(double  angle); 
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y  Hi  %  5f:  ^  if:  ^  Jj:  Hi  %  ^  ^  sfc  ^  5k  5k  Hi  5ic  :fc  :Jc  :f:  :f:  ^  :|c  :i:  jf:  4:  ^  ^  if!  :}:  J}:  ^  ^  Jf: 

Module  Name:  main  function 
Purpose:  for  symmetric  motion 
Parameters: 

Returns: 

Called  by: 

Calls: 

if:  H:  H:  ik  ik  5k  5k  ik  5k  ik  5k  5k  5k  ik  ik  ik  ik  5k  ik  ik  5k  H:  ik  ik  ik  *  ik  Ik  ik  ik  ^  ik  ^  ik  ik  5k  ik  5k  ik  5k  Jk  5k  ik  ik  ik  ik  5k  5k  *  5k  ik  ik  5k  ^  5k  ik  Hi  ik  ik  ik  ik  ik  5k  5k  5k^ 


main() 

{ 

CONFIGURATION  q,  L,  qstar; 
static  VirtualRobotMoving  =  1; 
static  RealRobotMoving  =  0; 
static  int  initialSetting  =  1; 
static  int  index  =  0; 
static  int  i,  Largestindex  =  0; 

/*  robot's  current  configuration  */ 

q.PositX  =  vehicle.Posit.X,  q.Posit.Y  =  vehicle.Posit.Y, 

q.Theta  =  vehicle.Theta,  q.Kappa  =  vehicle.Kappa; 

if  (initialsetting)  { 

/*  robot's  goal  configuration  */ 

L.Posit.X  =  path.Posit.X,  L.Posit.Y  =  path.Posit.Y, 

L.Theta  =  path.Theta  +  PI,  L.Kappa  =  -path.Kappa; 
initialsetting  =  0; 

} 

/*  store  the  goal  configuration  to  array  indexed  zero  */ 
symConfig[index].Posit.X  =  L.Posit.X; 
symConfig[index] . Posit.  Y  =  L.PositY; 
symConfig[index]. Theta  =  L.Theta; 
symConfig[index]. Kappa  =  L.Kappa; 

/*  1st  part 

move  real  robot  while  simulating  virtual  robot  */ 

while  (VirtualRobotMoving)  { 

q  =  moveRealRobot(q);  /*  move  real  robot  */ 

/*  compute  new  configuration  path  on  which  virtual  robot  moves, 
so  the  virtual  robot  L  tracks  the  line  defined  by 
configuration  q  */ 

L  =  calculateVirtualRobotPath(L,  q); 
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/*  store  new  simulated  configuration  to  array  */ 
index++; 

symConfig[index].Posit.X  =  L.Posit.X; 
symConfig  [index]. Posit.  Y  =  L.Posit.Y; 
symConfig  [index]  .Theta  =  L.Theta  -  PI; 
symConfig  [index]  .Kappa  =  -L.Kappa; 

/*  test  xstar  if  the  real  robot  passes  some  point 
where  the  real  robot  meets  the  virtual  robot  */ 

qstar  =  computeQstar(symConfig[index],  q); 
if  (qstar.PositX  <=  0.0)  { 

VirtualRobotMoving  =  0; 

RealRobotMoving  =  1; 

} 


/*  2nd  part 

make  real  robot  track  the  simulated  path  until  the  index  is  0  */ 

while(RealRobotMoving)  ( 
if  (index  >=  0)  { 

/*  test  xstar  if  the  robot  passes  local  target  configuration 
on  simulated  path  */ 

qstar  =  computeQstar(symConfig[index],  q); 

if  (qstar.Posit.X  >=  0.0)  { 
q  =  moveRobotOnSimulatedPath(q,  index); 

} 

else  index—; 

} 

else  { 

LEDon(4); 

currentPath.pathType.mode  =  STOPMODE; 
commanded.Linear  =  0.0; 
commanded.Rotational  =  0.0; 
initialsetting  =  1; 

} 

} 

} 
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«|I>  *1>  ^Tr*  *>t«^  *|t>  ^t-  fc-Tf  ^f|^  «1>  a  ^2^  fcjfj  ^tr*  *lr*  ^1^  S^  *1^  ^2^  *2^ 

/ ^J?  ?|v  ^|>  Jjv  i|f»  Jj5  #J»  ?f5  ?|C  JJ5  ?J5  ^J»  ^y5  ^y»  ^X*  ^X*  ^T*  ^m*  ^T*  ^T*  ^r* 


Module  Name:  moveRealRobot  function 

Purpose:  move  real  robot  step  forward 

Parameters:  q 

Returns:  q 

Called  by:  main 

Calls:  circ(),  compositeO 


CONHCURATION 
moveRealRoboKCONHGURATION  q) 
{ 

CONFIGURATION  deltaQ; 
double  dtheta; 

dtheta  =  0.0; 

deltaQ  =  circ(deltaS,  dtheta); 
q  =  composite(q,  deltaQ); 
q.kappa  =  0.0; 

return  q; 

} 
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Ji: :}:  :f: :J:  :ic  5f:  ^  sfj  i{<  5k  5fi  ^  Hi  iff  ^  5^  Hi  ^  5k  iff  ^  Jfs  Hi  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^ 


Module  Name:  calculateVirtualRobotPath  function 
Purpose:  track  the  virtual  robot  to  the  reference  line 
Parameters:  q,  L 
Returns:  q 
Called  by:  main 

Calls:  getKappaO,  circ(),  composite() 

*  H:  Hi  ik  *  5k  5k  5k  5k  5k  *  ik  5k  ik  5k  *  *  5k  *  5k  ik  H:  5k  H:  5k  5k  *  5k  Hf  5k  *  5k  :k  ik  5k  ik  iff  5k  5k  if:  5k  5k  5k  5k  5k  5k  5k  5k  5k  ik  5k  5k  5k  5k  5k  ik  5k  5k  5k  ik  5k  iff  5k  5k  5ky^ 


CONHGURATION 

calculate VirtualRobotPathCCONHGURATION  q,  CONFIGURATION  L) 

{ 

CONFIGURATION  deltaQ; 
double  dkappa; 
double  theta; 

double  DifferentDeltaS  =  0.2; 

L.theta  =  L.theta  +  PI; 

dkappa  =  q. Kappa  +  getKappa(q,  L)  *  DifferentDeltaS; 
theta  =  DifferentDeltaS  *  dkappa; 
deltaQ  =  circ(DifferentDeltaS,  theta); 
q  =  composite(q,  deltaQ); 
q.Kappa  =  dkappa; 

retvun  q; 

} 
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^tr^  ^t|^  A-f^  ^t>  4.1a  ^Ta  ^a  ^Ia  ^A  4^  «|1^  «1^  ^1*  ^l*' 

/  ^ji  wf*  ^j%  ^1?  ^f%  ^j>  r|C  ij>  ij?  ^f%  ^J?  ^1?  ?J>  ^J?  ^J>  ^5  ^f*  wf*  wf^  ^pt  ^y*  ^y*  ^y»  ^X*  ^X* 


Module  Name:  moveRobotOnsimulatedPath  function 
Purpose:  track  the  real  robot  on  simulated  path 
Parameters:  q,  i 
Returns:  q 
Called  by:  main 

Calls:  computeSymmetricPathKappaO,  circ(0,  compositeQ 


CONHCURATION 

moveRobotOnSimulatedPath(CONFIGURATION  q,  int  index) 

{ 

CONFIGURATION  deltaQ; 
double  dkappa,  dtheta; 

dkappa  =  q.Kappa  +  computes ymmetricPathKappa(q,  index)  *  deltaS; 
dtheta  =  deltaS  *  dkappa; 
deltaQ  =  circ(deltaS,  dtheta); 
q  =  composite(q,  deltaQ); 
q.kappa  =  dkappa; 

return  q; 

} 
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Module  Name:  computes  ymmetricPathKappa  function 
Purpose:  compute  kappa  for  real  robot  on  simulated  path 
Parameters:  q,  i 
Returns:  kappa 

Called  by:  moveRobotOnSimulatedPath() 

Calls:  computeQstarO 


double 

computes  ymmetricPathKappa(CONFIGURATION  q,  int  i) 

{ 

CONFIGURATION  qstar; 

double  deltaKappa,  deltaTheta,  deltaD; 

qstar  =  computeQstar(q,  symConfig[i]); 

deltaKappa  =  q.Kappa  -  symConfig[i]. Kappa; 
deltaTheta  =  q.Theta  -  symConfig[i]. Theta; 
deltaD  =  qstar.PositY ; 

return  -(aa  *  deltaKappa  +  bb  *  norm(deltaTheta)  +  cc  *  deltaD); 

} 
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«X*  ^L*  ^tl^  «!•*  *1^  *S^  «!*  ^L*  \1*  ^X<*  *1^  ^L^  *J^  ^A»  *L*  S^  SLf 

/  ^j>  <f>  ^J5  ^J»  <J»  ^J>  5J>  JJC  Jy5  ^5  ^**  ^yi  ^y*  *T*  ^y*  ^X*  ^X*  ^X* 

Module  Name:  getKappa  function 

Purpose:  compute  kappa  for  virtual  robot 

Parameters:  q,  L 

Returns:  kappa 

Called  by:  main 

Calls: 


double 

getKappa(CONFIGURATION  q,  CONFIGURATION  L) 

{ 

double  deltaD; 

deltaD  =  -(q.Posit.X  -  L.Posit.X)  *  sin(L.Theta)  + 

(q.Posit.Y  -  L.PositY)  *  cos(L.Theta); 

return  -(a  *  q.Kappa  +  b  *  (q.Theta  -  L.Theta)  +  c  *  deltaD); 

} 
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c. 


C  PROGRAM  FOR  INITIAL  MOTION  PLANNING 


jf:  ^  5j«  ^  %  :f:  Jk  Jf:  %  ^  iis  5^  5k  4!  %  ^  ^  Pk  5k  Hi  Hi  He  Hi  5k  5k  ^  % 


Module  Name:  Header  file 
Purpose:  define  variables 
Parameters: 

Returns: 

CaUed  by: 

Calls: 

Hi  H:  >1: 5k  Jk  5k  5k  5k  Hi  5k  5k  5k  5k  5k  5k  Jk  5k  5k  ik  5k  Hi  5k  >k  Hi  5k  5k  Hi  5k  5k  Hi  5k  5k  5k  5k  5k  Hi  Hi  Hi  Hi  Hi  5k  5k  5k  5k  5k  5k  5k  5k  5k  Jk  5k  5k  5k  5k  Hi  5k  Hi  5k  Jk  5k  5k  5k  5k  5k  Hi/ 


#include  <iostream.h> 

#include  <math.h> 

#define  PI  3.1415926 

#define  initialSigma  20 

#define  ABS(x)  ((x  <  0.0)  ?  -x :  x) 

typedef  struct  { 
double  X; 
double  Y; 

}POINT; 

typedef  stmet  { 

POINT  Posit; 
double  Theta; 
double  Kappa; 

ICONHGURATION; 

/*  define  robot  size  */ 

POINT  comer [4]; 

CONFIGURATION  robotCorner[4]; 
CONHGURATION  foreRunner[  1000000]; 

double  sigma,  k,  a,  b,  c; 
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/*  define  functions  used  in  program  */ 

CONFIGURATION  determineDirectionOfMotion(qs,  qe); 
void  getConstantsforForerunner(double  sigma); 

CONHOURATION  runForerunnerCCONHGURATION  q,  CONFIGURATION  qe); 

double  getKappaCCONHGURATION  q,  CONFIGURATION  L); 

int  checkIfcollide(CONnGURATION  q); 

double  calculateAngle(CONFIGURATION  q,  int  i,  int  j); 

int  checkIfQparallelQE(CONHGURATION  ql,  CONFIGURATION  q2); 

int  checkIfQequalsQE(CONFIGURATION  q); 

/*  library  functions  */ 

CONHGURATION  computeQstar(CONFIGURATION  ql,  CONFIGURATION  q2); 
CONnGURATION  composite(CONRGURATION  ql,  CONFIGURATION  q2); 
CONFIGURATION  circ(double  deltaS,  double  theta); 
double  norm(double  angle); 
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Module  Name:  main  function 
Purpose:  for  initial  motion 
Parameters: 

Returns: 

CaUed  by: 

CaUs: 


main() 

{ 

CONFIGURATION  q,  qs,  qe,  qd,  qstar; 
int  foreRunnerMoving  =  1; 
int  index  =  0,  midPoint; 
int  i; 

double  firstSigma,  dinit; 
sigma  =  initialSigma; 

FILE  *  stream; 

/*  for  printing  simulated  path  */ 
stream  =  fopen("data",  "wt"); 

if(stream  ==  NULL)  fputs("File  not  found!\n",  stderr),  exit(l); 

/*  robot's  size  */ 

robotCorner[0].Posit.X  =  -25.0,  robotComer[0].Posit.Y  =  30.0; 
robotCorner[l].Posit.X  =  25.0,  robotComer[l].Posit.Y  =  30.0; 
robotCorner[2].Posit.X  =  25.0,  robotCorner[2].Posit.Y  =  -30.0; 
robotCorner[3].Posit.X  =  -25.0,  robotComer[3].Posit.Y  =  -30.0; 

/*  robot's  initial  and  goal  configuration  */ 

qs.PositX  =  50.0,  qs.Posit.Y  =  100.0,  qs.Theta  =  -3  *  PI  /  4.0,  qs.Kappa  =  0.0; 
qe.Posit.X  =  200.0,  qe.Posit.Y  =  700.00,  qe.Theta  =  PI  /  2.0,  qe.Kappa  =  0.0; 

/*  region  where  robot  starts  moving  */ 
comer[0].X  =  0.0,  corner[0].Y  =  0.0; 
comer[l].X  =  0.0,  corner[l].Y  =  700.00; 
comer[2].X  =  200.00,  comer[2].Y  =  700.00; 
comer[3].X  =  200.00,  comer[3].Y  =  0.0; 

qstar  =  computeQstar(qs,  qe); 
dinit  =  qstar.Posit.Y; 
qstar.PositX  =  0.0; 

qd  =  composite(qe,  qstar); 
qd.Theta  =  qe.Theta; 
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/*  store  the  initial  configuration  to  array  indexed  zero  */ 
foreRunner[index].Posit.X  =  qs.Posit.X; 
foreRunner[index]. Posit.  Y  =  qs.Posit.Y; 
foreRunner[index]  .Theta  =  qs.Theta; 
foreRunner[index]. Kappa  =  qs.Kappa; 

/*  set  the  current  configuration  to  start  configuration  */ 

q.PositX  =  qs.Posit.X; 

q.PositY  =  qs.Posit.Y; 

q.Theta  =  qs.Theta; 

q.Kappa  =  qs.Kappa; 

/*  set  constants  with  sigma  */ 
getConstantsforForerunner(sigma); 

while  (foreRunnerMoving)  { 
q  =  runForerunner(q,  qd); 
index++; 

/*  store  the  new  configuration  to  array  */ 
foreRunner[index].Posit.X  =  q.PositX; 
foreRunner[index]. Posit.  Y  =  q.PositY; 
foreRunner[index]. Theta  =  q.Theta; 
foreRunner[index].  Kappa  =  q.Kappa; 

if  (checklfcollide(q)  ==  1)  { 
sigma  =  sigma  /  2.0; 
if  (sigma  <  0.095  *  dinit)  { 

printfC'Sorry,  no  possible  path  to  avoid  collision!\n"); 
exit(O); 

} 

index  =  0; 

q.Posit.X  =  qs.PositX; 
q.Posit.Y  =  qs.Posit.Y; 
q.Theta  =  qs.Theta; 
q.Kappa  =  qs.Kappa; 

getConstantsforForerunner(sigma); 

} 

if  (checkIfQparallelQE(q,  qd))  { 
firstSigma  =  sigma; 
midPoint  =  index; 
foreRunnerMoving  =  0; 

} 
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}  /*  end  of  while  loop  */ 

foreRunnerMoving  =  1; 
sigma  =  initialSigma; 

while  (foreRunnerMoving)  { 
q  =  runForerunner(q,  qe); 
index++; 

/*  store  the  new  configuration  to  array  */ 
foreRunner  [index]. PositX  =  q.Posit.X; 
foreRunner[index] . Posit.  Y  =  q.Posit.Y; 
foreRunner[index]. Theta  =  q.Theta; 
foreRunner  [index]. Kappa  =  q.Kappa; 

qstar  =  computeQstar(qe,  q); 

if  (qstar.PositX  <=  0.0)  { 
if  (checklfQequalsQE(qstar)) 
foreRunnerMoving  =  0; 
else  { 

sigma  =  sigma  /  2.0; 
index  =  midPoint; 

q.Posit.X  =  foreRunner  [index]. PositX; 
q.Posit.Y  =  foreRunner[index].PositY; 
q.Theta  =  foreRunner  [index]  .Theta; 
q.Kappa  =  foreRunner[index].Kappa; 

getConstantsforForerunner(sigma); 

} 

} 

}  /*  end  of  while  loop  */ 

} 
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Module  Name:  determineDirectionOfMotion  function 

Purpose:  determine  the  direction  of  robot  moving 

Parameters:  qs,  qe 

Returns:  qs 

Called  by:  main 

Calls:  norm 

^2#  «!.•  a1*  ^l»  «!>*  ^I' *1^  ^X^  sLf  ^t>  ^1^  ^X*  ^tf  / 

Jj5  <J>  ^1%  ^|y  #J»  <|C  #JC  J|^  /JJ  JJ?  ¥fi  fj>  ¥f9  ^9  ^1%  #7»  ^1%  rfj*  ^7*  t 


CONHCURATION 
determineDirectionOfMotion(qs,  qe) 

{ 

if  (ABS(norm(qs.Theta  -  qe.Theta))  <=  PI  /  2.0)  { 
if  (qs.Theta  -  qe.Theta  >  PI  /  2.0)  qs.Theta  =  qs.Theta  -  2  *  PI; 
else  if  (qs.Theta  -  qe.Theta  <  -  PI  /  2.0)  qs.Theta  =  qs.Theta  +  2  *  PI; 

} 

else  if  (ABS(norm(qs.Theta  -  qe.Theta))  >  PI  /  2.0)  { 
if  (qstar.Posit. Y  >=  0.0)  { 

if  (qs.Theta  -  qe.Theta  >  0.0)  qs.Theta  =  qs.Theta  -  2  *  PI; 

else  if  (qs.Theta  -  qe.Theta  <  -  2  *  PI)  qs.Theta  =  qs.Theta  +  2  *  PI; 

} 

else  if  (qstar.Posit.Y  <  0.0)  { 

if  (qs.Theta  -  qe.Theta  <  0.0)  qs.Theta  =  qs.Theta  +  2  *  PI; 
else  if  (qs.Theta  -  qe.Theta  >  2  *  PI)  qs.Theta  =  qs.Theta  -  2  *  PI; 

} 

} 

return  qs; 

} 
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*******  ***************** 

Module  Name:  getConstantsforForerunner  function 
Purpose:  compute  constants  a,  b,  c  with  new  smoothness 
Parameters:  sigma 
Returns: 

Called  by:  main 
CaUs: 

***:(:****<:**************************************4:**H=**************/ 


void 

getConstantsforForerunner(double  sigma) 

{ 

/*  constant  for  steering  function  used  in  simulation  */ 
k  =  1.0  /  sigma; 
a  =  3  *  k; 
b  =  3  *  k  *  k; 
c  =  k  *  k  *  k; 

} 
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Module  Name:  runForerunner  function 
Purpose:  simulate  the  robot  with  arbirtary  sigma 
Parameters:  q,  L 
Returns:  q 
Called  by:  main 

Calls:  getKappaO,  circQ,  composited 


CONHGURATION 

runForerunner(CONFIGURATION  q,  CONHGURATION  L) 

{ 

CONFIGURATION  deltaQ; 
double  dkappa; 
double  theta; 
double  deltas  =0.1; 

dkappa  =  q.Kappa  +  getKappa(q,  L)  *  deltaS; 
theta  =  deltas  *  dkappa; 
deltaQ  =  circ(deltaS,  theta); 
q  =  composite(q,  deltaQ); 
q.Kappa  =  dkappa; 

return  q; 

} 
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Module  Name:  getKappa  function 

Purpose:  compute  kappa  for  virtual  robot 

Parameters:  q,  L 

Returns:  kappa 

Called  by:  main 

CaUs: 


double 

getKappa(CONFIGURATION  q,  CONFIGURATION  L) 

{ 

double  deltaD; 

deltaD  =  -(q.Posit.X  -  L.PositX)  *  sin(L. Theta)  + 

(q.Posit.Y  -  L.PositY)  *  cos(L.Theta); 

return  -(a  *  q.Kappa  +  b  *  (q.Theta  -  L.Theta)  +  c  *  deltaD); 

} 
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%  :ic  He  ^  ^  5k  Jfi  Jf:  JjJ  ^  >f:  %  sis  iff  Jis  sK  iii  5k  %  Hi  *  He  if:  Hi  Jfi  JfJ  Hi  5f<  Jfi  Jfi  He  Jf:  sfi  if:  Jic  sK 


Module  Name:  checklfcollide  function 
Purpose:  test  the  robot’s  colliding  of  wall 
Parameters:  q 
Returns: 

Called  by:  main 
Calls:  caculateAngleO 


He  Hi  H:  He  He  Hi  He  ik  He  Hi  5k  He  He  He  He  He  He  He  He  He  He  He  He  He  He  He  He  He  Hi  He  He  He  5k  He  5k  5k  Hi  He  He  He  He  He  H;  He  He  He  He  He  He  He  5k  He  He  He  He  He  He  He  He  He  5k  He  He  He  He  j 


int 

checkIfcollide(CONFIGURATION  q) 

{ 

intij; 

double  angle  =  0.0; 
CONFIGURATION  qCorner[4]; 


qCorner[0]  =  composite(q,  robotCorner[0]); 
qCorner[l]  =  composite(q,  robotCorner[l]); 
qCorner[2]  =  composite(q,  robotCorner[2]); 
qCorner[3]  =  composite(q,  robotCorner[3]); 

for  (i  =  0;  i  <  4;  i++)  { 
angle  =  calculateAngle(qCorner[i],  3, 0); 
for  (j  =  0;  j  <  3;  j++)  { 

angle  =  angle  +  calculateAngle(qCorner[i],  j,  j  +  1); 

} 


if  (angle  >  -PI)  return  1; 

} 


return  0; 

} 
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Module  Name:  calculateAngle  function 

Purpose:  calculate  the  view  angle  of  robot 

Parameters:  q,  i,  j 

Returns:  angle 

Called  by:  checkIfcollide() 

CaUs: 


double 

calculateAngle(CONFIGURATION  q,  int  i,  int  j) 

{ 

return  norm(atan2(corner|j].Y  -  q.Posit.Y,  comer|j].X  -  q.Posit.X)  - 
atan2(corner[i].Y  -  q.PositY,  comer[i].X  -  q.Posit.X)); 

} 


54 


>}:  jf: ^  5}«  *  *  sis  *  J*:  *  *  ^  *  if:  sk  *  JfJ  iff  Jf;  Jf:  Hi  *  *  *  *  Hi  5|c  :f:  :fi  :ii: 


Module  Name:  checklfQparallelQE  function 
Purpose:  test  the  robot  is  parallel  to  the  reference  line 
Parameters:  ql,  q2 
Returns: 

Called  by:  main 
Calls: 

J 


int 

checklfQparallelQECCONFIGURATlON  ql,  CONFIGURATION  q2) 

{ 

double  dtheta; 


dtheta  =  ql. Theta  -  q2.Theta; 

if  (dtheta  >=  -0.1  &&  dtheta  <=  0.1)  return  1; 

return  0; 

} 
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Hi  5k  5k  Hi  ^  ^  5k  5ft  ^  Hi  5k  Hi  JfJ  ^  Hi  JfJ  5k  ^  Hi  ^ 

Module  Name:  checklfQequalsQE  function 
Purpose:  test  the  robot  gets  to  the  qe 
Parameters:  q 
Returns: 

Called  by:  main 
CaUs: 

********:t:********=l:**************H:********************************/ 


int 

checkIfQequalsQE(CONFIGURATION  q) 

{ 

if  (q.Posit.Y  >=  -0.01  &&  q.Posit.Y  <=  0.01)  return  1; 
return  0; 

} 
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D.  C  LffiRARY  FUNCTIONS 


Module  Name:  computeQstar  function 

Purpose:  calculate  the  local  coordinate  between  two  configurations 
Parameters:  ql,  q2 
Returns:  qstar 
Called  by: 

CaUs: 


CONFIGURATION 

computeQstar(CONFIGURATION  ql,  CONFIGURATION  q2) 

{ 

CONFIGURATION  qstar; 

qstar. Posit.X  =  (ql.PositX  -  q2.Posit.X)  * 
cos(q2.Theta)  + 

(ql.PositY  -  q2.Posit.Y)  * 
sin(q2.Theta); 

qstar, Posit. Y  =  (ql. Posit. Y  -  q2.Posit.Y)  * 
cos(q2.Theta)  - 
(ql.PositX  -  q2.PositX)  * 
sin(q2.Theta); 

qstar.Theta  =  ql. Theta  -  q2.Theta; 

return  qstar; 

} 
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^  :f:  jjc  :f;  >f:  ^ 5|c  :f:  >}c  sf:  :Jc  :f:  :fc  :t:  ^  ^ ^  sfj  5^  5k  ^  5f:  :f: :}::}« >f:  :f: :}:  jf:  :f: Hi  ^  ^ 

Module  Name:  composite  function 
Purpose;  compose  two  configurations 
Parameters:  ql,  q2 
Returns: 

Called  by: 

CaUs: 

^:k5j:5k*5{c5k5f::k**Jk5f:5j«5k:f:5k:fi5k*5k5k:k5i:Jic5H***************************************/ 


CONHGURATION 

composite(CONFIGURATION  ql,  CONFIGURATION  q2) 

{ 

CONFIGURATION  q; 

q.Posit.X  =  ql.PositX  +  q2.Posit.X  *  cos(qLTheta)  - 
q2.Posit.Y  *  sin  (ql. Theta); 
q.Posit.Y  =  ql. Posit. Y  +  q2.Posit.X  *  sin(ql. Theta)  + 
q2.Posit.Y  *  cos(ql  .Theta); 
q.Theta  =  ql. Theta  +  q2.Theta; 

return  q; 

} 
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^  ^  ^  sf:  Hi  Hi  5i!  5k  Hi  5fc  5l<  JfJ  Jff  *  5{< ijj  J}«  sf:  >i<  *  Jfj  5{«  Hi  5f: :}:  *  5k  5ft 


Module  Name:  circ  function 
Purpose:  get  configuration 
Parameters:  deltaS,  theta 
Returns:  q 
Called  by: 

Calls: 


CONHGURATION 
circ(double  deltaS,  double  theta) 

{ 

CONFIGURATION  q; 

q.PositX  =  (1.0  -  theta  *  theta  /  6)  *  deltaS; 

q.Posit.Y  =  (1.0  /  2.0  -  theta  *  theta  /  24.0)*  theta  *  deltaS; 

q.Theta  =  theta; 


return  q; 

} 
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Module  Name:  norm  function 
Purpose:  normalize  orientation 
Parameters:  angle 
Returns: 

Called  by: 

Calls: 

:f::f:5lj:fj:tt5};*4:*****5fi***************************************************/ 


double 

norm(double  angle) 

{ 

while  ((angle  >=  PI)  II  (angle  <  -PI)) 

{ 

if  (angle  >=  PI) 
angle  -=  2  *  PI; 
else 

angle  +=  2  *  PI; 

} 

return  angle; 

} 
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